#include <tf/tf.h>
#include "control/move_control/move_fsm.h"

using namespace control;
using namespace FSM;

void MoveFsm::Init()
{
    // 重置状态机
    FSMReset();

    // 设置默认状态
    task_id_ = 0;
    task_time_ = ros::Time::now();
    SetCurState("Finish");

    // 底盘控制状态机:
    // 1. 当初始状态, 为Finish, task_id = -1
    // 2. 当接收到任务, 为Work, path_following开始计算期望速度下发给驱动
    // 3. 当完成任务后, 为Finish
    // 4. 当暂停时, 为Pause
    // 5. 当继续时, 为Work
    // 6. 当异常或取消时, 为Fail
    StateRegist("Finish", &MoveFsm::FinishState);
    StateRegist("Work", &MoveFsm::WorkState);
    StateRegist("Pause", &MoveFsm::PauseState);
    StateRegist("Fail", &MoveFsm::FailState);

    StateTransitionRegist("Finish", "Work", &MoveFsm::IsFinishToWork);

    StateTransitionRegist("Work", "Finish", &MoveFsm::IsWorkToFinish);
    StateTransitionRegist("Work", "Pause", &MoveFsm::IsWorkToPause);
    StateTransitionRegist("Work", "Fail", &MoveFsm::IsWorkToFail);

    StateTransitionRegist("Fail", "Work", &MoveFsm::IsFailToWork);

    StateTransitionRegist("Pause", "Work", &MoveFsm::IsPauseToWork);
    StateTransitionRegist("Pause", "Fail", &MoveFsm::IsPauseToFail);
}

void MoveFsm::InitFinishPoseThreshold(float tofinish_dis_precision, float tofinish_angle_precision)
{
    fsm_tofinish_angle_precision_ = tofinish_angle_precision;
    fsm_tofinish_dis_precision_ = tofinish_dis_precision;
}

void MoveFsm::EventHandle(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    // set last fsm state
    last_state_ = cur_state_;

    for (int iter = 0; iter < regist_order_[cur_state_].size(); iter++)
    {
        if ((this->*(state_transition_map_[cur_state_][regist_order_[cur_state_][iter]]))(task, amr_state))
        {
            MoveState(regist_order_[cur_state_][iter]);
            StateHandle(cur_state_, amr_state);
            break;
        }
    }
}

void MoveFsm::StateHandle(const std::string &state, control::AMRState &amr_state)
{
    (this->*(state_action_table_[state]))(amr_state);
}

void MoveFsm::FinishState(control::AMRState &amr_state)
{
    MoveState("Finish");
}

void MoveFsm::FailState(control::AMRState &amr_state)
{
    MoveState("Fail");
}

void MoveFsm::WorkState(control::AMRState &amr_state)
{
    MoveState("Work");
}

void MoveFsm::PauseState(control::AMRState &amr_state)
{
    MoveState("Pause");
}

// TODO
bool MoveFsm::IsFinishToWork(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    // 开始工作的条件:
    //  1. 接收到原地自旋, 或同步自旋, 或移动任务

    if (amr_state.target_state.is_receive_cmd)
    {
        task_id_ = amr_state.target_state.move_task_id;
        task_time_ = ros::Time::now();
        return true;
    }
    else
    {
        return false;
    }
}

bool MoveFsm::IsWorkToFinish(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    if (task_id_ != amr_state.target_state.move_task_id)
    {
        task_id_ = amr_state.target_state.move_task_id;
        task_time_ = ros::Time::now();
    }

    if (is_force_to_finish_)
    {
        LINFO("force stop");
        return true;
    }
    
    bool just_in_stop = ton_amr_still_.Q(fabs(amr_state.actuator.angle_speed) < g_zero_speed && fabs(amr_state.actuator.walking_speed) < g_zero_speed, 100);
    bool just_in_point_id = (amr_state.control_state.cur_point_id == amr_state.target_state.target_point_id);

    if (amr_state.msg_trajectory_task.points.empty())
    {
        return just_in_point_id;
    }

    common_lib::Common common_ca;
    float diff_angle = common_ca.AngleConstraintToPI(amr_state.msg_trajectory_task.points.back().amr_angle - tf::getYaw(amr_state.amr_local_.pose.orientation));
    bool just_in_angle = (fabs(diff_angle)< fsm_tofinish_angle_precision_);
    bool just_in_last_index = (fabs(amr_state.msg_trajectory_task.points.size() - amr_state.min_index - 1) < 3);

    if (just_in_stop && just_in_point_id && just_in_angle && just_in_last_index)
    {
        return true;
    }
    return false;
}

bool MoveFsm::IsWorkToPause(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    // 暂停工作的条件:
    //  1. 调度下发暂停
    //  2. 调度下方警告
    //  3. 遥控器或按钮为急停
    //  4. 故障诊断下发暂停

    if (amr_state.actuator.emergency_btn||
        amr_state.event == Event_e::emc_stop)
    {
        // 保存上一时刻故障诊断结果, 为继续任务作判断
        last_event = amr_state.event;
        last_stop_bnt = amr_state.actuator.emergency_btn;
        return true;
    }
    else
    {
        return false;
    }

    
}

bool MoveFsm::IsWorkToFail(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    if (!amr_state.actuator.is_auto_mode) 
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool MoveFsm::IsPauseToWork(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    // 继续工作的条件
    //  1. 调度下发继续工作
    //  2. 或者当前 amr_state.event 为 normal, 且上一帧 amr_state.event 为 emc_stop
    //  3. 上一次按下急停按钮，且要求当前急停按钮取消，当前事件为normal
    if ( // task.cmd.cmd == "start" ||
        (amr_state.event == Event_e::normal && last_event == Event_e::emc_stop) ||
        (amr_state.event == Event_e::normal && !amr_state.actuator.emergency_btn && last_stop_bnt))
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool MoveFsm::IsPauseToFail(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    return IsWorkToFail(task, amr_state);
}

bool MoveFsm::IsFailToWork(const amr_trajectory_msgs::amr_trajectory_msg &task, control::AMRState &amr_state)
{
    return IsFinishToWork(task, amr_state);
}

int MoveFsm::GetTaskId()
{
    return task_id_;
}

ros::Time MoveFsm::GetTaskTime()
{
    return task_time_;
}
